import math
import time

import cv2
import mediapipe as mp
import numpy as np
from PyQt5.QtCore import pyqtSignal, QObject


def vector_2d_angle(v1, v2):
    '''
        求解二维向量的角度
    '''
    v1_x = v1[0]
    v1_y = v1[1]
    v2_x = v2[0]
    v2_y = v2[1]
    try:
        angle_ = math.degrees(math.acos(
            (v1_x * v2_x + v1_y * v2_y) / (((v1_x ** 2 + v1_y ** 2) ** 0.5) * ((v2_x ** 2 + v2_y ** 2) ** 0.5))))
    except:
        angle_ = 65535.
    if angle_ > 180.:
        angle_ = 65535.
    return angle_


def hand_angle(hand_):
    '''
        获取对应手相关向量的二维角度,根据角度确定手势
    '''
    angle_list = []
    # ---------------------------- thumb 大拇指角度
    angle_ = vector_2d_angle(
        ((int(hand_[0][0]) - int(hand_[2][0])), (int(hand_[0][1]) - int(hand_[2][1]))),
        ((int(hand_[3][0]) - int(hand_[4][0])), (int(hand_[3][1]) - int(hand_[4][1])))
    )
    angle_list.append(angle_)
    # ---------------------------- index 食指角度
    angle_ = vector_2d_angle(
        ((int(hand_[0][0]) - int(hand_[6][0])), (int(hand_[0][1]) - int(hand_[6][1]))),
        ((int(hand_[7][0]) - int(hand_[8][0])), (int(hand_[7][1]) - int(hand_[8][1])))
    )
    angle_list.append(angle_)
    # ---------------------------- middle 中指角度
    angle_ = vector_2d_angle(
        ((int(hand_[0][0]) - int(hand_[10][0])), (int(hand_[0][1]) - int(hand_[10][1]))),
        ((int(hand_[11][0]) - int(hand_[12][0])), (int(hand_[11][1]) - int(hand_[12][1])))
    )
    angle_list.append(angle_)
    # ---------------------------- ring 无名指角度
    angle_ = vector_2d_angle(
        ((int(hand_[0][0]) - int(hand_[14][0])), (int(hand_[0][1]) - int(hand_[14][1]))),
        ((int(hand_[15][0]) - int(hand_[16][0])), (int(hand_[15][1]) - int(hand_[16][1])))
    )
    angle_list.append(angle_)
    # ---------------------------- pink 小拇指角度
    angle_ = vector_2d_angle(
        ((int(hand_[0][0]) - int(hand_[18][0])), (int(hand_[0][1]) - int(hand_[18][1]))),
        ((int(hand_[19][0]) - int(hand_[20][0])), (int(hand_[19][1]) - int(hand_[20][1])))
    )
    angle_list.append(angle_)
    return angle_list


def h_gesture(angle_list):
    '''
        # 二维约束的方法定义手势
        # fist five gun love one six three thumbup yeah
    '''
    thr_angle = 65.  # 手指闭合则大于这个值（大拇指除外）
    thr_angle_thumb = 53.  # 大拇指闭合则大于这个值
    thr_angle_s = 49.  # 手指张开则小于这个值
    gesture_str = None
    if 65535. not in angle_list:
        if (angle_list[0] > thr_angle_thumb) and (angle_list[1] > thr_angle) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] > thr_angle):
            gesture_str = "0"
        elif (angle_list[0] > thr_angle_thumb) and (angle_list[1] < thr_angle_s) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] > thr_angle):
            gesture_str = "1"
        elif (angle_list[0] > thr_angle_thumb) and (angle_list[1] < thr_angle_s) and (angle_list[2] < thr_angle_s) and (
                angle_list[3] > thr_angle) and (angle_list[4] > thr_angle):
            gesture_str = "2"
        elif (angle_list[0] > thr_angle_thumb) and (angle_list[1] < thr_angle_s) and (angle_list[2] < thr_angle_s) and (
                angle_list[3] < thr_angle_s) and (angle_list[4] > thr_angle):
            gesture_str = "3"
        elif (angle_list[0] > thr_angle_thumb) and (angle_list[1] < thr_angle_s) and (angle_list[2] < thr_angle_s) and (
                angle_list[3] < thr_angle_s) and (angle_list[4] < thr_angle_s):
            gesture_str = "4"
        elif (angle_list[0] < thr_angle_s) and (angle_list[1] < thr_angle_s) and (angle_list[2] < thr_angle_s) and (
                angle_list[3] < thr_angle_s) and (angle_list[4] < thr_angle_s):
            gesture_str = "5"
        elif (angle_list[0] < thr_angle_s) and (angle_list[1] > thr_angle) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] < thr_angle_s):
            gesture_str = "6"
        elif (angle_list[0] < thr_angle_s) and (angle_list[1] < thr_angle_s) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] < thr_angle_s):
            gesture_str = "7"
        elif (angle_list[0] < thr_angle_s) and (angle_list[1] < thr_angle_s) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] > thr_angle):
            gesture_str = "8"
        elif (angle_list[0] < thr_angle_s) and (angle_list[1] > thr_angle) and (angle_list[2] > thr_angle) and (
                angle_list[3] > thr_angle) and (angle_list[4] > thr_angle):
            gesture_str = "good"
    return gesture_str


def get_line_decline(two_points_line):
    k = - (two_points_line[1][1] - two_points_line[0][1]) / (two_points_line[1][0] - two_points_line[0][0])
    result = np.arctan(k) * 57.30

    return result


class HandPostureDetect(QObject):
    handSignal = pyqtSignal(int)
    gestureSignal = pyqtSignal(str)

    def __init__(self, parent=None):
        super(HandPostureDetect, self).__init__(parent)
        print("[INFO] initalizing Handposture Detector...")
        self.mp_drawing = mp.solutions.drawing_utils
        self.mp_hands = mp.solutions.hands
        self.hands = self.mp_hands.Hands(static_image_mode=False,
                                         max_num_hands=1,  # 设置最多检测手部数量为1
                                         min_detection_confidence=0.75,
                                         min_tracking_confidence=0.75)
        self.degree_list = []

    def detect(self, frame):
        try:
            # frame = imutils.resize(frame, width=720)
            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            frame = cv2.flip(frame, 1)

            start = time.time()
            results = self.hands.process(frame)
            time.sleep(0.046)
            end = time.time()

            during_time = end - start
            # print(during_time)

            frame = cv2.cvtColor(frame, cv2.COLOR_RGB2BGR)

            if results.multi_hand_landmarks:
                for hand_landmarks in results.multi_hand_landmarks:
                    self.mp_drawing.draw_landmarks(frame, hand_landmarks, self.mp_hands.HAND_CONNECTIONS)
                    hand_local = []
                    for i in range(21):
                        x = hand_landmarks.landmark[i].x * frame.shape[1]
                        y = hand_landmarks.landmark[i].y * frame.shape[0]
                        hand_local.append((x, y))

                    # 判断动态手势
                    two_points_line = [hand_local[8], hand_local[7]]
                    degree = get_line_decline(two_points_line)
                    degree = round(degree)
                    self.degree_list.append(degree)
                    if len(self.degree_list) > 20:
                        del (self.degree_list[0])

                    for i in range(len(self.degree_list)):
                        if self.degree_list[i] < -75 or 75 < self.degree_list[i]:
                            self.degree_list[i] = 0
                    # print(self.degree_list)

                    count = 0
                    for i in self.degree_list:
                        if i < 0:
                            count += 1

                    count_1 = 0
                    for i in self.degree_list:
                        if i > 0:
                            count_1 += 1

                    if self.degree_list[0] == 0 and self.degree_list[1] == 0 and self.degree_list[
                        2] != 0 and count > 13:
                        self.handSignal.emit(1)

                    if self.degree_list[0] == 0 and self.degree_list[1] == 0 and self.degree_list[
                        2] != 0 and count_1 > 13:
                        self.handSignal.emit(2)

                    if hand_local:
                        angle_list = hand_angle(hand_local)
                        gesture_str = h_gesture(angle_list)
                        # print(gesture_str)
                        self.gestureSignal.emit(gesture_str)

                        # 在图像上写出识别结果
                        cv2.putText(frame, gesture_str, (0, 100), 0, 1.3, (0, 0, 255), 3)

                    else:
                        gesture_str = ''
                        self.gestureSignal.emit(gesture_str)
        except:
            frame = frame

        return frame


if __name__ == '__main__':
    cap = cv2.VideoCapture(0)
    A = HandPostureDetect()

    while True:
        _ret, frame = cap.read()
        # frame = cv2.flip(frame, 1)
        frame = A.detect(frame)
        cv2.imshow('1', frame)
        cv2.waitKey(33)
